coxa=14.8;
femur=45.45;
tibia=59.29;
base=92;

%P=[72.42,0,49];
%P=[69,0,75];
%PO=[69,0,67];

% This is the standard move forward/backward
% This is combined with running y-axis (k/2 -> -k/2)
% 	For varying y-value
%	for s = 51 : -5 : -51; 
% 		Varying y-value. Step forward.
%		PO(1,2) = s; % y goes from 51 to -51
PO=[51,51,71];

% This is the side move (crab)
% It must be combined with running x-axis, fixed y
% 	For varying x from -80 - -30 / 30 - 80
%	for s = 95 : -5 : 10; 
%		PO(1,1) = s; % y goes from 51 to -51
PO=[51,0,74];
	
%P=[75,0,79];
signum_arms=[-1,1,1,-1];

direction_angle=input("Set angle to rotate the direction vector (0-90 deg) :");
%direction_angle=12;
drr=direction_angle*pi/180;
direction=[cos(drr),-sin(drr);sin(drr),cos(drr)];

for noha = 1:1:4;
	switch(noha)
	case(1)
		theta=45;
		tr=theta*pi/180;
		R=[cos(tr),-sin(tr);sin(tr),cos(tr)];
		figure();
	case(2)
		theta=315;
		tr=theta*pi/180;
		R=[cos(tr),-sin(tr);sin(tr),cos(tr)];
		figure();
	case(3)
		theta=225;
		tr=theta*pi/180;
		R=[cos(tr),-sin(tr);sin(tr),cos(tr)];
		figure();
	case(4)
		theta=135;
		tr=theta*pi/180;
		R=[cos(tr),-sin(tr);sin(tr),cos(tr)];
		figure();
	otherwise
		R=[1,0;0,1];
	endswitch;

	disp("[x,y,z] tip:");
	disp(PO);
	
   P = [signum_arms(noha)*PO(1,1),PO(1,2)]*direction;
   P(1,3) = PO(1,3);

	disp("Prerotated [x,y,z] tip:");
	disp(P);

%	temp = [signum_arms(noha)*P(1,1),P(1,2)]*R;
	temp = [P(1,1),P(1,2)]*R;
	RP = P;
	RP(1,1)=temp(1,1);
	RP(1,2)=temp(1,2);
	
	disp("Rotated [x,y,z] tip:");
	disp(RP);
	
	A=p2a(RP);
	disp("Angles:");
	disp(A);
		
	P1=a2p(A);
	disp("3x [x,y,z] for all servo pivot points");
	disp(P1);
		
	a=linspace(0,P1(1,1),10);
	b=linspace(0,P1(1,2),10);
	c=P1(1,3).*ones(1,10);
	plot3(a,b,c,"w");
	axis([-150,150,-150,150,0,150],"square");
	title('3DOF Simulation')
	xlabel('x'); ylabel('y'); zlabel('z') 
	hold on;
		
	a=linspace(P1(1,1),P1(2,1),10);
	b=linspace(P1(1,2),P1(2,2),10);
	c=linspace(P1(1,3),P1(2,3),10);
	plot3(a,b,c,"w");
		
	%a=linspace(P1(2,1),P(1,1),10);
	%b=linspace(P1(2,2),P(1,2),10);
	%c=linspace(P1(2,3),P(1,3),10);
	%plot3(a,b,c,"m");
		
	a=linspace(P1(2,1),P1(3,1),10);
	b=linspace(P1(2,2),P1(3,2),10);
	c=linspace(P1(2,3),P1(3,3),10);
	plot3(a,b,c,"w");
	
	% For varying y-value
	%for s = 51 : -5 : -51; 

	% For varying x from -80 - -30 / 30 - 80
	for s = 95 : -5 : 10; 
	
		% Varying y-value. Step forward.
		%PO(1,2) = s; % y goes from 51 to -51

		PO(1,1) = s; % y goes from 51 to -51

	%	P(1,2) += 5;
	%	P(1,3) -= 0.8;

		disp("[x,y,z] tip:");
		disp(PO);

      P=[signum_arms(noha)*PO(1,1),PO(1,2)]*direction;
      P(1,3) = PO(1,3);

      disp("Prerotated [x,y,z] tip:");
      disp(P);

%		temp = [signum_arms(noha)*P(1,1),P(1,2)]*R;
		temp = [P(1,1),P(1,2)]*R;
		RP = P;
		RP(1,1)=temp(1,1);
		RP(1,2)=temp(1,2);
	
		disp("Rotated [x,y,z] tip:");
		disp(RP);
	
		A=p2a(RP);
		disp("Angles:");
		disp(A);
		
		P1=a2p(A);
		disp("3x [x,y,z] for all servo pivot points");
		disp(P1);
		
		a=linspace(0,P1(1,1),10);
		b=linspace(0,P1(1,2),10);
		c=P1(1,3).*ones(1,10);
		plot3(a,b,c);
		
		a=linspace(P1(1,1),P1(2,1),10);
		b=linspace(P1(1,2),P1(2,2),10);
		c=linspace(P1(1,3),P1(2,3),10);
		plot3(a,b,c,"g");
		
	%	a=linspace(P1(2,1),P(1,1),10);
	%	b=linspace(P1(2,2),P(1,2),10);
	%	c=linspace(P1(2,3),P(1,3),10);
	%	plot3(a,b,c,"m");
		
		a=linspace(P1(2,1),P1(3,1),10);
		b=linspace(P1(2,2),P1(3,2),10);
		c=linspace(P1(2,3),P1(3,3),10);
		plot3(a,b,c,"r");
		drawnow;
	end;
	hold off;
end;
